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The tracking problem today is of great interest particu- 
ieee tie Militery area. The term “tracking” is considered 
to be the processing of measurements obtained from a target 
Mmomade rn tO Maintain an estimate of its current state™ The 
Saeco Usually contains: 

a. Kinematic components (position, velocity, acceleration, 
See 3) 2 

b. Other components (radiated strength of signal, spec- 
Cmcimetemaecteris tes, ClC.). 

c. Constant or slowly-varying parameters (propagation 
Mwoleetty, coupling coefficients, etc.) . 

Peeeadek 15 a State trajectory estimated froma set of 
measurements which are associated with the same target. 

Measurements are noise-corrupted observations related 
to the state of the target. For example a measurement can 
Pemetrece Cstamatre Of DPOSition, range, bearing, time differ- 
ence of arrival, frequency of narrow band signal, or frequency 
@iitesence Of arrival. 

Many approaches have been developed for the solution of 
the tracking problem. Many of them are very reliable and 
work with satisfactory results. The problem however 1s 
complicated, so there is always need for further improvement 


of the tracking procedures. 


"14 


The complexity of the tracking @enesb tems cu ow eee 
following main reasons: . 

a. Presence of countermeasures (evaSive maneuvers, 
decoys, cre 

b. Need of advanced information processing techniques 
for manipulation of the numerous and complex data to 
be possible. 

c. Uncertainty associated with the measurements related 
to the origin of the measurements, l.e., a measure- 


ment may not have Originated from the Panger of 11 eeneoces 


ale Inaccurate measurements becamse Of LPanmeom nen sce. 
e. Random false alarms invehe detecureneeneeecsce 
ae Clutter because of reflectors or radiators near the 


target of interest. 
g- Interfering taggers: 
The basic components of a tracking system are shown in 
BG eels 
The term "filter" is often applied to devices or systems 
which process incoming signals or other data in such a way 
as to eliminate noise, to smooth®signals® or to predict wene 
input signal from instant tO instant. ~ Paeremeeerrc meee ee 
ture covering the theories of estimation, identification, 
modeling, prediction, etem@ee The des#gn Cf Suet cerenr ae 
in the domain of optimal filteringyewntenscmie nat cam yaaa 
the work of Wiener [Ref. 1] and was extended by the work of 


Kalman-Busy [Ref. 2] and Others. It was Only sarcuna los 
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that the systematic treatment Of Crae mimo mult tolew Garage. 
in the presence of false alarms using Kalman filtering 
techniques has started with the work of [Ref. 3] and [Ref. 4]. 

It has been generally accepted that the Kalman filtering 
technique is one of the most desirable of existing tracking 
algorithms. The reason is that i Pegeeob l=) eeacece OD wien, 
inputs (bearing, range, Course, Speedy eoot-=, cre.) nea 
inputs can be available from 4 Vaeuep ee (e647 7ons ena 
sensors. The Kalman filter then weights them properly with 
respect to their errors and generates an estimate of target 
position, course, and speed. It contavms alsouanrormacron 
within its structure Sufficient tO Gl Vemrnewisem sor Gectoten 
maker a useful indmcacor so. aye estimate's quality. 

The purpose of this paper 1s to describe the target 
tracking problem under noisy Conditiomes. Sie Kalman. seo 
is used as the basic tool to treat this problem. Some char- 
acteristic problems are presented and solved. 

In Chapter II some basic concepts and definitions are 
provided from estimation theory. | The feontenmae ee) soeenap— 
ter form the theoretical background needed for the rest of 
the chapters. It 1S assumed that the reader is familiar 
with basic probability theory. 

Chapter III contains a general description of the Kalman 
filter (discrete and continuous) and the nonlinear case 
where a linearization of target tracking problem takes place 
for the application of Kalman filter to be possible (Extended 


Kalman Filter). 


ang 


iimeideremny, a brief analyois of the error covariance 
matrix and the information contained in it has been made. 
Also its use in setting error ellipsoids (confidence regions) 
about an estimated target position has been described. 

In Chapter V the maneuvering target problem is described 
and one of the various existing approaches is analyzed, 
named "The White Noise Model with Adjustable Level." The 
same method is used later in Chapter VI, in a relative exam- 
eres Chapter VI contains three characteristic examples 
evening the application of Kalman filter in some individual 
Gases Of target tracking problems. Specifically the presented 
examples are the following: 

a. Linear target problem (measurements of target's posi- 
tion available). 

b. Non-linear target problem (measurements of target's 
bearing available). 

c. Linear maneuvering target's problem (measurements of 
position available). The target maneuvers two times 
with different acceleration each time. 

The relative calculations are provided analytically 
together with the corresponding computer programs. The 


resulting plots are also provided with proper comments. 
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II. SOME DEFINITIONS 42hiibee eae Cee. 
FROM ESTIMATION AEeRy 
The following definitions and concepts from probability 
and estimation theory respectively are described in detail 


in Reference 5 and Reference 6. 


A. Da Nit PONS 
1. Bayes‘ Rule for Random Variables 


For random variables Bayes' rule is given by: 


! SB eee P(y!x) P(x) 
ee P(y) | CYL XA eee eles ee 
The conditional probability of an event is sometimes referred 
to as "posterior" while the unconditional one as "prior." 
In this case p(x) 1s called the “orior "srebab tc e-1 se. 
function (p.d.f.) and p(x/y) is the posterior one. 


Ze GausSian Random Variables 


The p.d.f. of a GausSian random variable is 





—,2 
2 2 
p(x) = N(k;)0+)— =e a, COP) 
/270 20 
where N( ) represents the normal density with argument x, 
mean x and variance o. 
An equivalent expression is 
= 2 
x) Lae No (23) 


weece xX 1S normally distributed with respective mean and 


variance. 
3. White Noise 
A discrete Gaussian random process v is called "white 


Merse if for any kK time points t,.t ae the random 


i k’ 

vectors eG 7 oy VG) (which are Gaussian) are independent. 
The autocorrelation of this random process is zero for any 
two cirtferent times. 

A discrete Gaussian random process is often used 4s 
a model for measurement noise and disturbance noise. 

4. Markov Processes 

The common property of Markov processes is the 

meee wing : 


eae) x(t), T<t ) J 7 eee 


eS oe x(t 


Mmpeewmeele base Up £O ££, 15 completely described by the value 
Srethne process at t. 


The state of a dynamic system with input a white-noise 


~ 
ctr 
iI 


fete ere ce) | (235) 


moet MarKwOy Process. 
5. Random Sequences 
A random sequence, or a discrete time stochastic 


process, is a time-indexed sequence of random variables. 


20 


A random sequence is Markov if 


~] 


pix(k)|x2] = plx(k)|x(j)]  ¥ k > 3 (2. 


The sequence Vv(j), j = 1,3 jets eoeteere eee ce Lar 


la V k V S * 25 8 ) 


where Oi 1s the Kronecker detta funeeron: 


The state of a dynamic system excited by white noise 
x(ktl) = tftikeeteees (2.208 


is a discrete-time Markov process or Markov sequence. 


A stochastic process x(t), t ¢€ I is a Gaussian white 


(Or independent) process if for any m time points t aa 


Lecce 


yep e «oe eee 


in I (m = integer), the m random n vectors x(t - 


1 
are independent Gaussian random vectors. 
The state of a linear dynamic system excited by white 


Gaussian noise 


Za 


eT) | Ee agile ey x) (eas) 


Memomealss-Markov process. 


Pee bast CONCEPTS IN ESTIMATION 
1. Estimating Problem--Random and Non-random Parameters 


The problem of estimating a (time-invariant) parameter 


xe 1S 
Given the measurements 
Zee eh we) eee = 1, ore 
made in the presence of random noises w(j), find a function 
i ae k 
Cemex (K, Zan eo 13) 
where 
k 
Z SZ aa) pa lee | (Zena) 


that estimates the value of x in some sense. 
miemrmncelon (2.38 1s Called an estimator and its 
value is called the estimate. We can use two models for a 
Eeme-invariant parameter: 
a. Non-random: We have an unknown true value Xo Here 


1t is desired that the estimates converge in some sense to 


this true value as k + © (non-Bayesian approach). 


ee 


b. Random: The parameter is a random variable with a 
prior p.d.f. p(x). In this case a realization of x accord- 
ing to p(x) 1S assumed to have Calsemyeiha ee elec reed 
for each measurement sequence to vield an estimate that 
converges to the corresponding realization of x and this 
should hold for all x (Bayesian approach). 

In the second approach given che ertemeo-o.f> Gian 
parameter, its posterior can be obtained using the Bay's 
rule. 


In the first Gase Ehe: likeliiece Suieeter 


1s used as a measure of how "likely" a parameter value 1s 
for the observations that are made. 

A usual method of estimation of non-random parameters 
1s the maximum likelihood method. LES maccimum Tikelinece 


estimate (MLE) is 


x 9 (kK) =" arqemax p(Z* |x) (2s) 
x 


The corresponding estimate for a random parameter is 


the maximum a posteriori (MAP) estimate 


x (Kk) s=) Saicemex p (x | 2%) arc max [p (2*|x) p(x) ] G2 as 
x x 


Z° 


~ ~ 


“MAP 2a ML , 
The x wi Iycolnemeeewith x Pema veCm@ealn) OrlOr 


weer, Called "diffuse." 
2. Linear Estimation--Static Case 
The minimum mean square error (m.m.s.e.) estimate of a 


? 


random variable x in terms of another random variable y 
is the conditional mean E[xj;jy]. In practice the evaluation 
Oe eEhe conditional mean is complicated so usually the "linear 
Memes Ge eStimation” method is applied. According to this 
method the best estimate is such that it is unbiased and tne 
Peeimartion error is uncorrelated from. the observable(s), 
mioweemeney areomorthogonal. 

Hhemwestimatton Of EWO randemeveccors Xvand z jOlmcly 
normally (Gaussian) distributed 1S now examined: 


Let 


a 
y = (Zeeas) 
Ss 
The random variable y is normally distributed with mean 
a 
y = pron 
es 


and covariance matrix (assumed nonsingular) 
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E E 


Se ge 
E = (2 
ae 
EP E 
2%) ee 
where 
= =. El (x =e (2 
and 
Pip = El(x-x)(z-z')] (2 


Then the m.m.S.e. GSEIimMate Of Kein Cerms vous 


x = Bes eee: Pz Pes a= a (2 
and the corresponding Govaerancer fata. es 
c : E[ (x oo eee = P wie a e (22 
xR |Z t= i <XX  -TKZ ZZ ~Zx 


» 20) 


ie 


es 


24) 


We can see that the optimal estimate is a linear function of 


z (this is because of the Gaussian assumption), while the 
covariance which measures the "quality" of the estimate is 
independent Of Che vobsem amieimar 

If the random variables are not Gaussian, then the 
"best linear” estimate Of x Enters rc ee oeroee 


is: 
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ieeetins.c. Matrix Corresponding to Equation (2.25) is given 
Dy 


Eel | = Pp - Pp P P =P 2 2G. 
S —XX —XZ —ZZ —ZX —XX Z 


Mmeeme the above results it follows that the best estimator 
(in the m.m.s.e. sense) for Gaussian random variables 1s the 
Same as the best linear estimator for arbitrarily distributed 
random variables with the same first and second moments. 
Seeeestimating with the “Least Squares" Method 
The least squares method is another common estimaticn 


Paeeeeaure [Or NnOn-rancom parameters. For measurements 
ZA ee) fp Xoo wey) C2ae2 


The least square estimate of x is 


Oj) “2S Pies jin tint) zal hij,x)1° (25.23) 


Equation (2.28) does not make assumptions about the noises 
w(j). If these noises are independent identical distributed 
(i.i.d.) zero-mean Gaussian random variables, then Equation 


(2.28) coincides with the MLE for these assumptions. 


26 


The corresponding case for random parameters is the 
minimum mean square error (M.m.s.e.) estimate 


PS ari = arg min El (x -x) 712°} (2529) 


x 


The solution to the vagengoe- 


ee = E[x| 2%] = | 9(x| 2%) Oe ( 2a”) 


where the expectation 1S with respect to the conditional 


DeiGe boas 
k P(Z* 1x) P(x) 
Pex oo) ere (2.2315) 
Paz, 
Ay Consistent EStimacors 


For a non-random parameter case, the estimator is 
called consistent if the estimate converges to the true 
value in some stochastic sense. Using the convergence in 


mean square, 


Lim E{ {x(k} = x)]1°} = @ (2.32) 
k +>co 
The expectation is 
care 
Ei (3679) ol ae ee (2a 


oy 


Requirement for 


in the random parameter 


The expectation here is 


the convergence of the estimator 


case (m.s. sense) iS 


OVEE zk and x. 
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“III. KALMAN FILTER AND APPLICATION TO TARGET TRACKING 


BASICS ON KALMAN FILTER 


A. 
Kalman filtering technique is very popular in target 
tracking applications. tI is basi@ally agmethod Givscoel via 


the problems of optimal filtering an@eeredterien aed 


according to Reference 1 is defined as: 


a. 


tered. 


Optimal filtering is the estimation of state vector 
X(t) from data Z(t) where 7 < €. 
Prediction is the estimation of a state VeCEOr Gane) 


at time t from data Cae) wee a eor 


In target trackime, so-o ene above problems aré eneoun. 


The Kalman filter is desirable because aS an esStima- 


tion model it has the following features: 


ae 


At time t, the filter generates an unbiased estimate 
Ki) of the state veccor X(t). ) (hae eae eee. 
expected value of the estimate is the value of the 
state vector at time t. 

The estimate is a minimum variance estimate (li.e., it 
has the property that its error covariance is less 
than or equal to that of any other linear unbiased 
estimate). 

The filter is recursive and it does not store past 


data. 


ZS 


eee doe atice, 15 linéar which simplifies calculations 
and lends itself to machine computations. Gelb [Ref. 


3] discusses the above features. 


Pee, DISCRETE KALMAN FILTER 


Assuming that we are dealing with a discrete time svstem 


ea the form: 


X(k+1) = o(k+1,k) X(k) + A(k+1,k) ulk) + W(k) eaves 


the following declaration must be done: 

"<5 ]4)" toe eecdeais- = oie OL) olven J; Wihiichn means 
Siti I eeeStimate Of X at time j given measurements at 
meres up tO and including time }." 

Then there are eight main components that make up the 
Kalman filter: 

cee rs eile stare VYeccor dt cime K, and X (Kk) 1s the 
state vector estimate which is an unbiased minimum 
variance estimate of the true state vector X(k) at 
fame  K-. 

PeeteweLuet@co/artame>e Matrix P(Kj™ is a Matrix repre- 
senting the covariance of the difference between the 


PeGemoeite voemwer K(k) and the estimate Ree) and 


can be expressed as: 
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The transition matrix 6(K) seuseaein thevrea leu aaeen 
of the state vector estimate at the present point, in 
time (k+lj|k), from the state vector estimate of a 

past point in time (k|k). It is also used im the ecatleq= 
lation of the error CcOVarlance Mate ee cime (kee 
using the error cOvarianGe Matai eae ee mci 

The measurement Vector 20K) Use ya cee oo snowmen. 
observation taken at time k and its Gomponents are 
linear combinations of the components of the state 
vector X(k) which have been ComppupEeds by a ieeecelaes 
Gaussian noise V(k) whose mean 1S zero and has a 
covariance matrix R(k). 

The covariance matrix R(k) associated with thesGausssa7 
noise that corrupts the observation or measurement at 
time k, must be provided by the user. It 1s the covari- 
ance of a sensor méasu®emen= Veer sii ee ocr ares 
a range sensor may have a measurement variance of 60 
yards. A measurement made with that sensor would have 
@ scalar value of 60 yards fom oe eee ee --n 
later, 1t is important that tae vadwme OEgk (xk) be 
accurate as it affects the performance of the filter. 
The conversion matrix C(K) descmiscseciem teacecon. 


binations of the components of the state vector X(k) 


Sel 


Silos 


which makes the components of the measurement vector 
Z(k). The relationship between the measurement vector 
4(k), the conversion matrix C(k), and the Gaussian 


mosey (ej rs: 


(jew kalmanwcain Matrix Gik) is instrumental in mini- 
mizing the difference between the estimate X (k) and 
Pacmot deem cCremen kK) eee Gct(k) 2s %chosen to Minimize 


the trace of the estimate error covariance matrix 
P(k) and is used to revise the estimate X (Kk) of the 
eceemerectOl es (Kk) adr pemerrorscovarlance Maerix P (Kk) 
Aa te lewebser vation Z2ik) has been made. 
The last component to be discussed is the system 
eymomies COVarlancemmatrix O(K). The assumption of 
Seen ne eces) USMiencdiamene) State vector X(t) exists 
in a random environment that is Gaussian with zero 
mean and acmamience Oe eas Se CMe Gy namics COVarie 
ance matrix must be input by the user, and its 
determination is important as it also affects the 
performance of the filter. 

The remainder of this subsection shows the relation- 
between the algorithm. Two important points follow: 
hen ohe ron dtr Ix eons), Measumement noise covari- 


ance matrix R(k), systems dynamics covariance matrix 


SZ 


Q(k) and the conversion MatmxeiGi iia Se eleemteme mi 
for. €achn Bela reer reee Cur 

b. The filter must be iniltializegwie see de siseuee ae 2c] 
the initial estimate X (k) and its associated estimate 
error covariance Matrix (Eiie) 7 eee ma Gages een one 
will require more observations for the algorithm esti- 
mate to converge near the value of the state vector. 

A Kalman filter iteration can be divided into phases: 
Prediction and Filtering. 

During the predictlon® phaser oie ae weer ems eS 
mate X(k+1{k) and its error cOva~ lance sma tia cmae ie Bie) ee 
updated from the previous value at time (kK;k) to the time 
(k+1'k) when the current measurement Z(k) is observed. 
During the same phase the system dynamics is introduced into 
the algorithm. Phe Goyarience maeri Ce mice cours meee ee 
system dynamics (environment) \fmem tame tome 
The update is performed by multiplying the estimate x (k|k) 
and the error covariance matrix P(k|k) by the transition 


Ma tales $ (k+1,k) as follows: 
X (k+l = WOXkKr ly) ea (3.63 
P(kt+l|k) = @o(kt1,k) PikikKy) Oil oor (32G) 


If the system has an input wu, eheweeas ese G eee wabove 


equations is extended as follows: 
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Bis) == K+) YR) pa) eee LK) (k) Seen | 


— 
— 


Jo 
[> 
|G 


wn 


During the filtering phase, the estimate X(k;k) of 
eooeaee VeC lor A(Ki{K) amd the error covariance matrix 
Peewee) pare revised, based onthe latest measurement Z({k) 
eee eeeoe time x. TOmae chis, the Kalman gain matrix 
Pe eeeroe ed rSt Computed using the error covariance matrix 
Paes) which was updated in the prediction phase. The 
Poel ance Matrix Rik) which 1s the covariance of the Gaussian 
ome ao ceect ated with the Latest measurement Z2{(K) and tne 
were wom matrix C(k) are also associated with the latest 


measurement. The sequence of computations during the filter- 


ing phase is: 


Oreeee—= P(kik-1) G'(K) [C(k) P(k{k-1)C'(k) + R(k)]~ 353) 
K(k|k) = x(k[k-1) + Gaon cnie) aceare ic) X(k| K-1) ] (3.9) 
P(k|k) = [1 - G(k) C(k)] B(k|K=1) (3.10) 
iiowieasuremene oLrealectlvon Covariance 1s 
eter = Clk+1)P(k+1|k)C*(k+1) + R(k+1) (Seeiniy) 


The innovation Or measurement residual is 
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v(k+l) = 2(ktl) = 2(ktlik) | (3.12) 
An important property Of @ieweonevaeiens is eciae 


they are an orthogonal sequence, 1.e., 


Elvyvad = i (Seales 


where 1s the Kronecker delta function. 


9% 5 

It must be noticed that, at every stage k, tne entire 
past 1S summarized by the "sufficient statistic " key and 
the associated covariance. The covariance equations are 
independent of the measurements. The covariance equations 
can thus be iterated forward offline. 

Figure 3.1 shows a schematic of the Kalman filter 

algorithm and relationships among the components described 


above. Reference 7 provides a detailed discussion of Kalman 


filtering and optimal estimation. 


C.. CONTINUOUS KALMAR SE Deirak 
In order to go from a discrete coma cenetnuet msc eem 
Of they foram: 


X(t) = F(t) X(t) + B(t) w(t) (3.14) 


Z(t) eC (ayer Xue yremn ates (3.15) 


SD 


4 OM O8FIee God td 


QIZHDOMyA ew YY 


P(k+L | k) = o( 


c 





Input for Time k+l 


egies 73). 


= 9(k+1,k)x(k|k) + A(k+1,k)u(k) 


REAL WORLD 
(not observable 


Pee 
by user) 


(Input from Time k) 






RD Keim kde Ole) 





VECTOR 


Measurement 


ak) 
Observed 





Schematic of the K.F. Algorithm 
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where W,V are zero mean, uncorrelated, white noise processes 
with covariance matrices Q and R respectively, we have to 
consider the situation at the 11a.) — eee ee 


At this limit we have the following equivalences: 


(ae 2 he eae (Srloe 

O(k) > BOSE eaee (3 

Riga hee (Sealey 
R(k) = E(V(k) V‘' (k))- 42S. 4 covariance mateikg a tele 

R(t) = EV (t) V pop) (ee (3.19 
1S a spectral density matrix. The covariance matrix Rice =e 
has infinite valued elements. To approximate the continuous 


white noise process by the discrete white noise sequence, 
the pulse lengths (At) may be shrinked and their amplitudes 
may be increased, such that R(k) + R/At. In other words, 
as At + 0, the discrete noise sequence tends to one of an 
infinite valued pulse of zéro duration) (sien ena emia 
under the "impulse" autocorrelacienetumer lone om han ser 
equal to the area R under the continuous white noise impulse 
autocorrelation function. 

With the above expressions in mind, the procedure is to 


write the proper difference equations and to observe their 


Su 


Bememeor in the limit as St > 0: Lt is assumed that R is 
menm-sincular, i.ée., R ~ exists. 
Finally the continuous Kalman filter equations are sum- 


marized as follows: 


a. System Model 


eee Se) Re) FBC) Wey; Wt) © N(0,Q(t)) (Sa 0 
b. Measurement Model 
Pepe) Cle) X(t) + Vit), Vit) ~ N(O,R(t)) 21) 
Cc Pike tal Condi tions 
oie) = Xp E((X(0) ~Xp) (X(0) -Xp) 1] = Po one 2) 
d. Assumptions 
eae exists 
e. State Estimate 
X(t) eet) x(t) a G(t) (Z(t) -Ct) x(t) 1, x (0) a i 2 3) 
£ Pmnermeovc Lance Propagation 
Poem eee (EE) P(ty) + P(t) Ftc) + Be) OWE ys) Gey Ste Rey tt) 
where P(0) = Py (5624) 
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g. Kalman Gatm Marri 


G(t) = [P(t)c'(t) + Blt)T(t)]R ~(t) (3. 26) 


For more details, See Reference7, pp. 119-124. 


ee EXTENDED KALMAT) Papier 

In target tracking it is usually needed to estimate the 
present target position, course and speed and to predict 
future target position based on the present estimate. The 
Kalman filter works very well for target tracking since the 
algorithm can provide an unbiased, minimum variance estimate 
of the target's state based on varied observations (filter- 
ing), predict future position using the prediction siasceuo-s 
the filter and provide an indicator of the estimate error 
through the estimate error covariance matrix. 

Usually in practice, the state and/or measurement equa- 
tions are not linear. Since the Kalman filter is applied to 
linear cases, it is necessary to find a "method" to use it 
in nonlinear estimation problems. One approach is to derive 
an Optimal filter for the nonlinear problem. Another apereaas 
(more usual) is to linearize the problem and apply Kalman 
Filter theory to the linearized problem. The highlights of 


the second method are presented in the following: 


ie 


The system and measurement equations are assumed to be 


of the form: 


Discrete 
SS (ek) ew tk) ene 
Paes Fe Ua) ay (k) Ze) 
Come inuous 
cay = coi en ee nt) (3.29) 
ae ee ee ty CE) (3310) 


Here it 1S assumed that we deal with a discrete model. 

It 1S assumed that we have available a trajectory 

Tae 0, 2 ene VEECOr flmetion aix@k) ,u(k) ,k) 

1s expanded in a Taylor series about the nominal trajectory 
(0) 

ne 


(k). Then the linearized state equations can be written 


ao. 


where A(k) is defined to be the first partial derivatives 


(0) 


Smeenecenomlinear function a(x Co, awe). whem respece 
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to the state vector x(k), LWeoe, 


es. SZ) 


The accuracy of this approximations decends om the Gircen- 
ence between x! 9) (x) and. the actudl (grejeetom 4 ee ea 
middie two terms are treated as deterministic inputs. 


Now the measurement equation has been considered. We 


have 


2(k) =aeregere vie (3.358 


The nonlinear vector o 1S expandedwagaln abou. eno sion ia 


Epa) eCCEOmy ee Then the measurement equation can be 
written as 

a (0) (0) 
20K) == Hie meres (ik) Sea (k) + v(k) (32. 349 


where H(kK) is defined as the first partial derivatives of 


the measurement function ete | cop with respect to the 
State Veccor 4am 
hes 
Aik = (32328 
ox x69) (4) 


As in the linearized state equation, the two middle terms 


are known time-varying quantities which can be handled as if 
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pee cre 2 time-varying measurement bias. For simplifica- 


mien lt 1S defined 


{> 
© 
© 


wu" (k) a(x’ ”” (k) ,u(k) sk) - A(k) x! (x) aaa 
Zi{k) = z(k) ~ e(x'’(k)) + B(x!) (K)) 

ome) = ek) Be oe 
So, chat 

x(k+1) = A(k)x(k) + u'(k) + w(k) (3.38) 

z'(k) = H(k)x(k) + v(k) (3.39) 


With these linearized equations, the appropriate Kalman 


filter equations are: 


a. The gain equation 
Gik) = P (k|k-1) H" (k) [H(k) P (k|k=1) H" (Kk) “ Bona (3.40) 
b. The covariance of estimation error equation 
Peegis le —  A(k-1) P(k=-l|k=1) A’(k-1) + Q(k-1) (e241) 
Eas ieee (k) Hic) 1 Ciice).) ero) 
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c. Filter update equation 


X(k[k) = X(kik-1) + G(k)(2(k) See Se (3.43) 
Gd.) Predict lom equa een 
x(Ktl|K) = «a (ete) (3.44) 


A block diagram of the system and estimator is shown in 
Pale ie wee. 
The gains can be pre-computed and stored 1f it is assumed 


that the nominal trajectory is known in advance. 


To answer the question, "where does the nominal trajectory 
x(k) come from?", two approaches are usually used. 
In the first, an approximate trajectory is used eras 


trajectory is known in advance from known data or may have 
been computer by solving the state equations 


x?) (ner) = atx?) cy ate) eR) (3.45) 


with the initial condition x!) (9) = B(X(0) 1 ele ee mune ome 
tainty in X(0) is large, the solution of the above equation 
may be "far" from x(k) to make the linearization Sutticrtemma, 
accurate. If an accurate nominal trajectory is available, 
the gain can be computed off-line and stored. 

In the second approach, the estimates produced by the 


filter are used as the nominal trajectory about which the 
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linearization is performed. The mMaeruees A(x) anc ee 


be used to generate G(k). The bese Vera ceror) acm awed 


available when H(k) must be evaluated is x(k,k-1); when A(k) 
is to be evaluated, x(k;k) is available. 


The filter that results from using Enis 2ecreaen = 
called an Extended Kalman Filter. The gain and covariance 
equations must be solved on-line. The procedure of the 
filter's calculations is given below: 


w~ 


1. Start with x(0;-1) and evaluate H(0) using: 


oc 
H(k) = 3% 0” Bates 
apes Us Teel) 
Zs 
P(O0[-1). = SYNE nl x (0) (0) eee (3.47) 
Use this matrix to compute G(0) given by 
G(k) = P(k|k-1)H' (k) [H(k)P(k{k-1)H' (k) + R(k)] >? (3.48) 
3 SGonpize = (Ole ron: 
X(k[k) = x(k|k-1) + G(k) [2(k) - ¢(x(k|k-1)] (3.49) 


ong 
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and 


the 


ane 


Paine) £L Om: 
egeriK) = a(x (k{k),u(k),k) 
SCo7eure Prdy0) from: 


P(k\k) + [I - G(k) H(k)] P(k 


<< 


1) 


LO 


eemeens 0M0)) is savailable before A(0) is calculated, 


value of x(0°0) is used, hence: 


3a 


OX 


then P(1|/0) is computed from: 


(x(0{0),u(0),0) 


Peep = A k~1)eyk-1)k-1)A'(k-1) -+ Q(k-1) 


Taylor series expansion up to the first order terms. 


UI 
(OO 


~o4) 


The process is repeated by recycling through steps 1 to 5. 


The Extended Kalman Filter presented was obtained by a 


This 


filter obviously introduces errors in the equations where the 


higher order terms are neglected. 


There are several ways of compensating for these errors. 
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Very roughly , an addition Of “areiticialjemes=—- 
noise" covariance can be made, for compensation of the errors 
is the state prediction. Also a multiplication of the state 
covariance by a scalar 1 > 1 at every sampling time can be 
carried out. This multiplication Is @q@uiyvallene towene yet iees 
having a "fading memory," l1.e., at every sampling time the 
past data is "discounted" by attaching tom] aug n-s cores. 
ance (lower accuracy). 

The above subject is covered with more details in Reference 


8, pp. 4-52--4.58 and in Reference 5, pp. 3.3=-l1-=-3. 4-1. 


47 


evs BeROrR COVARIANGE MATRIX AND TARGET 
TRAG@PNG OUALITY 
A. DEFINITION OF ERROR COVARIANCE MATRIX 
The error xX in the estimate of a state vector is defined 
bBemee the cdiftfterence between the estimated (2) ren ase 11 


actual (x) values: 
ee ee a (hd) 


UWiemamoove Girrerence 15 KnOwn as the error vector or 


estimate error. The covariance of the estimate error 15 
eee Epc Cee et) XC | (4122) 


Die weGeweeeco mas tacistiecal measure of the uncertainty in 
woeeltetonwpOssible to discuss the properties of the covari- 
ance matrix independently of the mean value of the state. 
Ticomiineciatton COmtained 1S Ssufficiemte to generate indicators 


of the estimate quality. 


Bt PmieOnrMArTOt CONTAINED IN THE ERROR COVARIANCE MATRIX 
ieGguaren hive important Characteristics of the error 
cOvariance matrix which relate the matrix to the state 
vector and its estimate. 
(1) The error covariance matrix of an n-component state 


eGo 15 an mM by mn Symmetric Matrix. 
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(2) The diagonal elements of the error covariance matrix 
are the mean square errors of the-error vector 
components. 

(3) The trace of the error covariance matrix is the 


mean square length of tne enue —eror, 


(4) The off-diagonal terms of the matrix are correlations 
between the elements of the error vector X(t) = Cee 

(5) The error covariance matrix Pik) Gemesree cme ss: eeem 
dynamics covarlance Maenix) © (gc oe eo oe 
infinity. This means that as More Iantboeumaete nes 


= 


available about the state vector (observations) the 
estimate uncertainty approaches the uncertainty of the 


environment in which the state vector eGCx1iSES. 


C. ERROR ELLIPSOIDS ABOUT A TARGETS Os i ae 

Frequently it 1s Significant to have available an dndieae 
tion of the quality of the estimates. One approach to 
achieve this is the proper use of the error covariance 
matrix P(ki|k). The outline of this appmoaenmeemceceigmece 
below. 

It is assumed that the initial state of the model x(0) 
and the random processes) vik) (0 w (5) Vamem Gaul sie eee 
assumption 1S satisfied then it can be shown that: 


: 


x(k) ,x(k|k) and = e(k|k) CK |) eee 


are Gaussian. The results obtained apply only to this case. 
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MMe probability density function for e(kik) can be 


Written as 


F 1 1 a 
fi(e(k;k)] = exp[- >-e'(k:k)P ~(k kije(k.x 
ia | my 2 | iy 2 = = = 
(27) / pet kos e 
anc 3) 
fem EExeca Lime K this @xpression can be written as 
fp fe) = Te 173 exp -Se'we (4.4 
C2 a) |e 
where w = p - (k|k). Mimeoucmeaconaecrenmmi ne the Locus Of 
points where the density function tole), has a COmstant 
value the above equation has to be examined. It is seen 
eal te Ep le) has a constant value whenever 
3 Seen COMscant = i (4.5) 


Reem eeS1OWWM tiat tne points © which satisfy Equation 
(4.5) are hyperellipsoids (in two dimensions, ellipses). 
If the left side of (4.5) is expanded for the two-dimensional 
case (the same approach can be extended to n dimensions), 


we have: 


e,e, +5 Wj, e5 = |] jc) 
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where the symmetry of w has been usec (Wi 5 o W})- EQuatiLon 
. 2 

(4.6) is an ellipse (W534 Oe Wo5 > Orraric Wi %59 > Wi>)- 

It is not easy to be recognized as such because its principal 

axes do not coincide with the coordinate axes as shown in 


Bigu ter cee 


First the principal axes must be determined and then the 





ellipse can be rewritten in terms of ve and ae as 
coordinate axes. 
a 
broanereal 
axis 
Dr imeLoat 
ax1s 


Figure 4.1 Error yeieiese 


Bak 


The ellipse in the new coordinate system is Gescribed bv 


2 2 


2 a 9 2 
r + = i  —-— Saas 
peeean 2" 2 1 eeaiesly a t (4.7) 
2 Z 

Ci) Le) , 
where y and y a ewe leh VCC coms (Or Wand = and ‘5 are 
the corresponding eigenvalues. 

As it has already been mentioned, W = oe ECU t 1 Oma. Bb) 

1s an ellipse in terms of the eigenvalues a and 5 ane ule 
elgenvectors a. and vine eee oe eS SlOn wmleh oi es 


the ellipse in terms of the eigenvalues and eigenvectors 


OL P is 
ys ye 
— + = re (4.8) 
He 2 
where yo iS an elgenvector for FP and a. = L/a, is the 


corresponding eigenvalue. 
This result can be generalized to n dimensions as well. 


Given the quadratic form 


= eieey teu) 1° (4.9) 
eaomelmojemvalies Of P are Ap rAgre essa, and the corresponding 
eigenvectors are ee See The guadratic form 
(Equation (4.9)) describes a hyperellipsoid which can be 


Wwrittenmas 


a 


y- ys yo 
14224 ..9 Je (4.10) 
a a ‘el 

il 2 n 


All vectors e in the n-dimensional space can be written as a 


linear combination of the eigenvectors gt) y (2 


THE Veeetriciene ~er vee in the linear Jeemetnatc2en. ts Yue 


the coefficient of y *) 1s Yo, etc. 

So the surfaces of equal probability density are 
ellipses (or hyperellipsoids). 

The problem can now be stated as follows: 

For a specified value of A, what is the probability thae 
e lies within or on the ellipsoid? 


These probabilities have been tabulated below for a 


few values of n and i. 


S22 Lea 


Probabilities Heme binwor etd oSemes 


d 
n a 2 3 
i Tose Poe moo 
Z ~394 7oGs .989 
e 5 (00, ie Bo ie 
For example, [Cr a4 Sys fem nay ner eee 


Probability error inside A = 1 ellipse = 0.394 


Probability error inside i 


2 ellipse U2so5 


ie 


Probability error inside A = 3 ellipse = 0.989 


Therefore, if the error covariance matrix P is given, the 
error ellipsoids can be determined by finding the eigenvalues 
and eigenvectors. 

The error ellipses are useful in visualizing the estima- 
tion error. By uSing them we can consider the true state 
Meee cOnllic within a certain region Surrounding the estimate. 

Peer decatls and derivations, see Reference 8, pp. 4.44-- 


ee) Ls. 
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V. MANEUVERING PARGE ES 


A. GENERAL DESCRIPTION OF DHE PRG@2r! 
The maneuvering target is generally described by the 


equation: 


where the matrices F(k), G(k) are assumed known and the 
process noise v(k) is zero mean, white random sequence with 
covariance Matrix O(K). THe Maim Charactcatsr (ee ae 
maneuvering target equation is that the inputs u(k) are 
unknown. 

In the following, linear models are examined for sim- 
plicity but the same techniques can be applied to nonlinear 
cases. 

A number of different approaches to the maneuvering target 
problem have appeared in the literature. The most commonly 
used model is, due to simplicity requirements, a kinematic 
model with states consisting of position, Vvelocic, sand. una 
most cases also acceleration, driven by white noise. 

It 1S possible to divide the different approaches into 
two broad categories: 

A. The unknown input (maneuver command) is modeled as a 
KaneGmiem EOCeSs. 


B. The unknown input is estimated in real time. 


5) 5) 


The random process type models can be further classified 

Meme Wwenecitogearles, depending ©m Eheilr statistical properties: 
Al. White noise 
AZ.  Autocorrelated (Markov) noise 

All these methods are approximations because in general, 
the maneuvers are not stochastic processes. 

In the input estimation case the assumption of having a 
BemseamG Input Over a Certain period of time is usually made. 
The estimation can be based on the least squares criterion 
and can be used in the following ways: 

Bl. The estimated input corrects the state estimate. 
B2. The input becomes an extra state component (state 
1S augmented) that iS reestimated within the state. 

Pero tOlLlowing section Only one method is described. 
This method is illustrated by an example in Chapter VI. It 
was selected from the others, due to its simplicity. 

For detailed descriptions of different approaches to the 


problem, see Reference 5. 


peepee WHITE NOISE MODEL WITH ADJUSTABLE LEVEL 

Pieents Meened a certain low level of process noise is 
aesumed in the filter. 

A maneuver 1S considered as a large innovation. The 
detection procedure is based on the square norm of the 


Pinieovatlons 


where 
V(k) -= z(k)) Sees eo) 


Based on the target model (for a non-maneuvering target) 


a thresnold is Set up 


where ais arbitrary. For example, a = 0.05. 

If the threshold is exceeded, O(k=1)" 1s tialriolicae, 
a SCadimiG. £ aGto imeem dead: Eu is reduced to the threshold 
“max 


When using the factor » the covariance of the innovations 


takes the form: 


S(k) = C(k) (F(k-1)B(k-1|k-1) 8! (kK-1) GG Ce 


(S58 


This obviously reduces the Valuecwete- ec 


V 


An analogous technique is possible to be used to lower 


the process noise level after the maneuver. 


- 


i 


PPC HARACTEREST. C OMPUTER: EXSMPLES 


A. A LINEAR KALMAN FILTER EXAMPLE 
esoolem Description 


The target is assumed to be described by the system: 


a0 


tI 





Sg Oo ee 


cs eel 
ke 
* 
x! 
II 
oO 
fies 
o 
tA 


| 
ee 


migememwik) ~ N(O,0), vik) ~ N(0O,R) are mutually independent, 
zero mean, white random sequences. 


Mac Tnltlal state 1s 


(Oe (6.3) 


ivemmoasuLementoe2to) ana 2(l) are made £Oo initialize 


a Kalman filter. 
The sampling time is given as T = 0.1. 
The process and measurement noise are to be yielded 


by a Gaussian random number generator, given that Q = R= 0.02. 


Be 


Case l 
After the run of the Kalmanwi beer toro) eee 
the following expressions are useful to be plotted: 


a. True trajectory vS. estimated Grajeecory 2meac Xy0X 


2 
plane. 
6b. Position error verlance 
Py (LL Bee eee 
G. Normalized  osition craen 
‘ WW 2 a 
dse Velocity errompiweriance 


ps5 (1|1), Poems cae ee 


e. Normalized velocity error 


ey 2 


[x5 (K) S25 OR oe enero 


P22 
ff. Normalized state error squared 


ae 


[x(k) -x(k|{k)]'P ~ (k) [x(k) -x(k]k)] 


g. Normalized innovation error 


[2 (k) -2(k]k-1)1/[p,1 (k[k-1) + RIO? 


a 


Case 2 
PiRekeGer ES see the ettece of running the Kalman 
Piece r with a Cterereme © Eham emac Of the model, it is helprul 
to plot the same expressions as in Case l, uSing a different 
OQ for the target's model, say Q = 9 lee 
Case 3 
Pimally, it is also interesting to see the change 
on the same expressions as the Q of both the model and the 
filter increases. For this reason new plots of these expres- 
Sions are to be obtained, using Q = 9 mo BRO Oot the mode: 
and the filter. 
2. Analysis 
ad. > True Trayeeeory 


LACmlaot mr Ort ob the Given model is converted 


mieceie f£Ollowing equations: 


x, (Kt1l) = x, (kK) + Tx, (k) + Tw(k) (OAs 
X5(K+1) = x, (k) + wk) omrey 
Bite —-0.), wik) 25 generated by a random number generator 
function and 
> 
0) 
i 
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bx Measurements 


From the given measurement equation it is 


obvious that 


Z2{k). = x, (kK) + as (Gn) 
where x (0) = 5 and v(k) is generated by a random number 
generator function. 

Cc. Derivation gem =e) eee 


It 1S reasonable to start with the following 


estimations for the position and velocity vectors: 


“A 


A x) (1) 1) Zale) 
ee A = 7 (6 ae 
x, (1) 1) ZAG eae 
eh 
then 
ey ee Ee ely (6.8) 
a prerem 8)) 
- iP 1 Vv (1) = 37200) 
xo | = ee 3 (6.9) 
Using these values in the initial error covariance 
matrix 


x4 (1|1) scope ae een 
P(l|/1) = ET | x 9 (6.10) 
x, (1/1) x, (1/1) Seely) 
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where 


as 


cvoweme= x) (1/1) = x yy) (6.11) 


|) cle eee (eli) (6.12) 


Herts Obtained that 





R 
ee 
P(1;1) cae? 


ee Run of the Kalman Filter 


Gimeeeca ciation OmeGainse= Ihe known equation 
Specie Kalman Filter is: 
P(k/k-1) = oP(k-1{k-1)o6' + Q' (k-1) (oe. 14) 


It has been converted in matrix form and after some manipula- 


tion, the following equations are obtained: 


k-1|k-1 : k-1|k-1 < eS 
eee kL) Dee (k=l |k-1) + T O(ky) (oars) 
ee oe =) Bikol kl) + TPS. (k-1|k-1) + TOQ(k) (6.16) 
epee Po (K-11) PES tk-1|k-1) + TQ(k) (Greg ) 


G2 


P55 (k | k-1) = P45 (k-1 k-1) + OS) | ere 
Keeping the same procedure, the Kalman Filter 
equation 
G(k) = P (k | k=1)¢" |e eaes| hres = R(k)]~> . (Ge 
takes the following form 
G,(k) = oh sll 6 
i ~ Pl, (ki|k-1) + R(k) i 
iv 
| k- 
G,(k) = 2. an (6 
2 Pos KK Sat aia 
il 
Finally the equation 
P(k|k) = [Ll =G@(eieie Goi Om 
is analyzed in the following: 
Py 5 (kk) = Pio (k|k-1) -G, (K)P,, (k|k-1) Ce 
= 2 a ieee 
PB, (k|k) Po, (k[k-1) - Go (kK) By y (kK) k-1) (on 
P55 (k|k) - Poo (k|k-1) - Go (k)P, 5 (k|k-1) CG 
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ney 


ry 


. 2a) 


22) 


23) 


24) 


253 


26) 


2. Estimated Trajectories 
Following the procedure described above,‘the estimated 


trajectories given by the equations: 


ee) = ¢ ee) 7 eels) foe2 7) 
x (elk) = x(k1k=1) + G(k) [z(k) - C(k) x (ki k-1)] (6.28 
take the form: 
x, (k/k=1) = x, (k=1[k-1) - 1h, bed ed) | (6229) 
x5 (Kk kK-l) = x, (k-1) k-1) (oc) 
x, («| K) - G, (Kk) z(k) ar (1-G(k) ) x, (k| K=1) oso.) 
x5 (k|K) = x, (k|k-1) *E Go (k) 2(k) = G, (k) x, (k|k-1) (Oo) 

©. Normalized State Error Squared 


The normalized state error squared 1S given by 


the expression: 


=i 


Pato x (kik) ] RP (Kk) ixtk) = x(k|k) 1 


By manipulating this expression in its matrix 


Pai lewls Obtained that 1€ 1s equal to the following: 
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i A 


BP 55 Pp iP 9 ( (x(k) “x (kK) 1°P5 + [xy (k)-x, (kK) K) I °P,, 
- 2P. 1 [x5 (k) x, (k) — x, (Kk) x, (K/K) ~ x(k, k) x, (x) 
+ x5 (k|k)x, (k|k) 1] 
ff. Normalized Innovation euge. 


By making use of the equation 
Z( kiko} =) eta) (6.3388 


the given expression for the innovation error becomes 


“w 
“ww 


iz(k = c(k| Ret 2) oO ees (eae 
iP, , (kik-1) +R}272 (Pee ee) eee ] 
Ll ot 
All the above have been properly set in a FORTRAN 
program (pp. 178-180). The results corresponding to cases 
1,2, and 3 are indicatéd on pages 66-09) eres econ = 


Some comments have been made on the computer program outputs 


On pages 65 and 90-94. 


5. Commences ene tiemcsacids 
a. Normalized Expressions ¢, e, g o©& Cases 1, 27 ane 


The normal GisStribution pasme. as: 





f(x) = = (6.35) 
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oy eancdarcai ze thnemnemmlal Aistrioution curve we 
Can Nave a single curve that may be adapted to all values of 
ereemeai as well as differing values of the eared Gey laclone 
This standardization may be accomplished by substituting 
me = x) /o in Equation (6.35) . 

| The above concept is the case for the subpara- 
maoeas ec, 6, Gg. In Case ¢c, for example, 
(k[k) ] ee 

The standard normal distribution curve has a 
mean of zero and variance and standard deviation equal to 
ene. 

By integrating Equation (6.35) (after the substi- 
meee Or 2), it 1s found that: 

Pewerwecme—-1O < Z =< flo, 68.2% of the area under the curve 
em ine Tuded , 

Between -20 < z < +20, 94.5% of the area under the curve 
fence ludec . 

Between -30 < z < +30, 99.74% of the area under the curve 
iomimcluded. 

With the above [mild yw ehe serOobability that the 
normalized position, velocity and innovation errors lie between 
Pao 15 94.5% (or between +30 is 99.74%). 

PEOlmetvemorapnis lc siscensthat im the two 


cases where the value of Q 1s common for the model and the 


20 


filter, the three normalized Grrensewietem0e- w>e-c.——. 
= 308 

In the case where 0 1S9@@ePerent sbetwesnieeg ae 
ance ela ener tic Cia aeeietees S905 that these errors exceed at some 
points +30. But generally it seems that the law of 935 a 
between +30 is applied satisfactory. 


b. True Trajectory vs. Estimated Trajectory 
(@Xpr. ca) 


(1) QO = 0.02 foreModet ander iter oo mec oe 
There is considerable difference between true and estimated 
curve at the beginning but as time progresses, there is a 
continuous improvement. Near the end, the two curves nearly 
coinerdes 

(2) Q = 0.02 fom che Filter and 0)— UFOs Siam 
the Model (pp. 74-75). There 1s a significant difference 
between true and estimated curve at the beginning which is 


slightly improved at the end of Gmegei ec. 


oy) Q = 0.09 for the Model and Filter “po. 622 eae 


Same as in (1), with the only difference that at the end of 
the curves, the improvement is not nearly a coincidence but 
it is better than that of Case (2). 

Generally the rate of the improvement is 
greater at smali’ values of time because the gain is inversely 


proportional to time (G(k) = 1/k+1) and 1 weights the correc. 


G 
tion term [z(k) - C(k)x(k|k-1) less heavily as time progresses. 
The fact that the improvement between true 


and estimated curves is less in Case (3) than that of Case (1), 


cial 


es 


Peron osecece. Ine reason is that © in Case (3) has a greater 
Pemeie ane aS™ic 1s known, QO increases the uncertainty in‘the 


See seem Drediction: 


{ 
|x 
Oo’ 
ow) 
O) 


{hy 
aa 
or 

il 
= 
| 
ep 

ae 
Q 
oun 

Me 
i 

x 
| 
k- 
Or 
Ge 


The fact that the estimated curve in Case 

Peet enrers trom the true curve more than that of Cases (1) 
Bees) ana also that the improvement is not so significant 
as in the other cases was also expected. The reason is that 
eiemOous Of che mode-and the filter are different, so this 
Meee emenece @erects the P and G of the filter negatively, 
mesbeing in a Gitfficulty in approaching the true trajectories. 

eee ermakeveeGe= orate Error seuared Error (expr. f£) 


Me 1S yne@ewn trom the theory that ae (ee 
is the sum of the squares of n independent zero-mean, unity 
Thabane Gaussian random variable, 1.e., N(0,1l). This means 


fe ant (Gee) P(x Se) hase ehi=souate distribution with n 
degrees of freedom (n is the dimension of vector x). 

Peeeiemenl-setarne distribution co equals zero, 
the true and estimated state vectors agree exactly. The 


larger the value of us the greater the discrepancy between 


eaemerie: ama estimated state vectors. 


oy 


What 1S expected to be seen in this problem is 
a greater discrepancy in the case where the Q's of the model 
and the filter are different, due to the poor estimations of 
the state vectors. 

The above expectation seems to be the case. 
Specifically in the cases where the same Q for the filter 


and the model is used, it Can be Save@ueete ee ceed. 


oe 
02 5 


be for two degrees of freedom (see proper tables). 


and coe are approximately 0.06 and 7.5 as they shou 


d. Position and Velocity Error Variances (exon. s7-8 

In Cases l and 2 (filter has Q = 0.02), we have 
identical position error variances an@eidenttea) voleere 
error variances. After some initial variations Chewoestemee 
error variance takes its steady state which is almost zero 
(expected), while the velocity error variance takes also its 
steady state which 1S approximately 0.04. 

In Case 3, the increase in Q, increases P,, (k,k-1) 


IESE 


and reduces Py, (kk) (see proper equations). Then the steady 


state values of P,, (k|k-1) and Pj 


the result is that the position error variance oscillates 


(k|k) are different and 


between the values 0.0 and 0.01. For the same reason the 
velocity error oscillates between the values 0.15 and 0.24. 

The mean values of both then are greater than the corresponding 
values of Cases 1, 2. This is logical Since now the Q is 


greater as it was derived: 


oe 


EeceevOnmearror, Jariance: 


Pry {Kik) = Pia (ki k-1) - G) (k) BP), (kj k-1) (6.38) 
Py, (ki k-2) = Piz fkrL k-1) + TP5, (k-1 (k-1) 
eo era) 4 1-o  (enao) 
=e ! 207 | ce EWilekseat(e 
Metocity Error Variance: 
, EO... ro aa a 
P55 (kK k) Gy (k)P,5(k\k ee P55 (k Keay) (6.40) 
P55 (k <i). = P55 (k-1)k-1) oT (6245) 
femoris Proportional to Pai and Po 5: 


Pee vee x~AMPLE USING &.K.F. (NON-LINEAR CASE) 


i ) Problem Description 


A stationary target is located at es es Oe 


Bearing measurements from a moving ship are taken at 


mews N from locations as indicated. 


The following are given: 


(k) + v(k) (6.42) 
i? 
aS aN eae. 
1 
0 (6.44) 
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Figure 6.25 Descriptive Diagram for the Non-linear Prope 


a(k) = 10k J =e N = l2 (6.463 


The Qnitial estumate sus: 


n~ 


. x, (0| 0) 80 
Cer ore = (6.47) 
x» (0|0) 120 
100 0 
P(0/0) = (6.48) 
0 100 


Based on the bearings of the moving ship, an Bxeenced 
Kalman Filter can be used to improve thewini tia ees a ane 


Of the tameet ss poster om: 


go 


Tt 1S useful to obtain the values of x\N Nj and P(N Nj 
ror observing the quality of the simulation. 
2. Analysis 


Since the target is stationary: 


; | 
x, (k+l) : x, (kK) | 
= | | (6.497 
eee Ko (K) | 
The model equations for non-linear problems are: 
Meee = alxikp,utk)e,k) sewik) (Seu 
ee Ctx (K)) =v (k) co) 
hoO@nenhs sOrob lem 
x (kK) 
Eee eK) y= One 22) 
ee 
SO 
OX, OX, 
IX, 3X. a 0 
3a 
A(k) = ee ) == = (Gys)3.) 
—' (x(k|k)) 
3X. 3X. 0 I 
IX, IX. 


lege eon Enis problem 15 given by: 


oS 


. el _ 
c(x(k)) = 3 = tan z an (6G 47) 
ib 
SO 
dc =a 
H — = cao = Pe (6235 50) 
=| oe) =| x (kee 
and 
-xX 
2 s 
2 
1 (x, -~al(k)) ~ x3 
x} -a(k) 
H = (6. sae 
2 
- (xy -a(k))* + x5 


To calculate the gains of the filter, the following 


equation Ls used: 


|@ 


(k) = P(k|k=-1)H' (k) 


[H(k)P(k|k-1)H!' (k) + R(k)] > (6.58) 
After some manipulations of the above equation in its matrix 


form, it is found that the gains are given by the following 


equations: 


Py, (k|K-1)H, (k) +P, 5 (k| k-1) Hy (k) 


2 2 
Hy (kK) P,, (k|k-1) +H, (k) 8 (k) [P51 (k|k-1) +P), (k|k-1) +H, (k) P,. (k| k-1) +R(k) ] 


Z 
(6.2519) 


OW 


» 9 Tl ’ s H 7 ae a 
1 % | oe ‘4 e el re | a ; 
(kK) H (x) [P,, (k|k 1) +P, 5 (k | k-1) +H, (k)P,. (Kk k-1)+R(x) | 


4 | 
Hy (k)P), (kjk-1) +H, (x) H, 


The estimation of the states is obtained by manipu- 


lating che following equation: 


BS) = X(K|K-1) + G(x) {z(k) =c(x(ktk-i))] Comer 
The results are: 
meow — xX, (kK) K=-1) + G. (k)z(k) - G,(k)tan —— 
ile a iP ald Ss 4 
x, (k | k-1) -a(x) 
(6.162) 
n a ST x5 (kK k-1) 
X5(kKi/k) = Xo (k|k-1) + Go (k)z(k) - G, (k) tan ~ 
x, (k/ k-1) - a(k) 


(Geo. 


ime Chrser Covariance Matrix during the filtering phase 


of the Kalman Filter is given by the equation: 
Fie) eae =a) (6.64) 


P(k/|k) = [I G(k) 


By manipulating this equation as it was described previously, 


the following equations are obtained: 


a3 


P11 (KIk) = Pi ,(k,k-1) [1 -G, (kK) Hy (Kk) | ~P,, (Kk, k-1)G, (k) H, (x) 
(63650 
Py > (kk) = [1 -G, (kK) H, (kK) JP), (k re Ik, ~ G, (kK) HB, (kK) P66 (k ale) 
(6. 51a7 
Po, (k/k) = <1 ~ Go (kK) Hy (kK) TPS, (Kk keer} ~ G, (kK) H, (kK) P,) (k k= 
(6.67) 
P55 (kk) = [1 - G, (kK) Hy (kK) ]P,,4(k Kea) —G, (kK) H) (kK) P),(k oe 
(O-61e0 
The error covariance matrix during the prediction 
phase of the Kalman Filter is given by the equation: 
P(k|[k-1) = A(k-1)P(k-1/k-1)A' (k-1) + Q(k-1) (6.69) 
This yields the following results: 
a ee Bay eee ae (6.70) 
Pe se) = Pk ea (Gcg 
Poy Kal) ee re (6.72) 


a2 


P.5(k,k-1) - P55 (K-1 i ow 


~~) 
La 


imemnormalized state error squared: 
Pee eee) |) Bk) x(k) = x(k7K) | 


is given by the same expression as in the previous problem. 

Pay the above results are used in a computer orogram 
(see Appendix A). This program basically follows the stecs 
of an Extended Kalman Filter algorithm as it was described 
Pie Chapter Lit. 

MEeMwrase OQUESUES It can be Seen that the position 
Pemeronce On the horizontal axis is smaller than that on the 
vertical axis. This was expected since the bearings are 
crossed with relatively small angles and in this way, there 
ioe larGer Umcertainty on the vertical axis. 

isomrrom the: results, Gt Can Bewseen that the esti- 
mates for X,71 X5, are improved as time goes on and finally, 


that they are very close to the real values. 


G. “MANEUVERING TARGET EXAMPLE 
Pee soorem Description 


The target is modeled by the following equation: 


Me = fete) Se Gow i(k) (6.74) 
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This equation is discretized over time intervals of Vengrn 


T. Using Cartesian coordinates, “ememeeatrer 1s: 
x = [x x vee (6.75) 
and 
il it O 0 
0 1 0 0 
F = 0 0 l 7 (6 7a 
0 0 0 a 
i = 
w = [Ww wo] (6:27 9a 
eZ 0 
iL 0 
G = 0 T/2 (6.76) 
0 l 
Elw(k)] = 0; Elwik))w(j)] “a Sea: (6. 70 


The initial estimate is x(0|0) with covariance P(0/|0). 
It 1S assumed that only position measurements are 


available: 
a) = C xtk) + wk) (6.60 


where 


ONS 


Cc = , 
Ca eee 
ieee) = 0; Sv vee) = Sse 
The following are given: 
fea os,) O = 0 R = R = 10 “a and 
, ia Ze ; 
7 2 
Ry 5 = 500m 
iMemimlttal conditions of the target are: 
page 200m, xi9) = 0, y(0) = 10,000m, y(0) 


foo 1) 


= Sms 


The target 1S on a constant course and speed until 


Pememwec = 4005S, when it maneuvers a slow 90° turn in the x 
Sirection with acceleration inputs us =u = ie on oe it 
completes the turn at t = 600s (after 20 sampling times) and 


Baoileea@em On the accelerations are zero. 

pesewen 90°, as fast. It starts at t = 
2 

of 0.3m/s and is completed at t = 660s, 


sampling times. 


1.e., 


after 5 


MmvemseeCOna  Lurn, 


610s with acceleration 


A Simulation of the maneuvering target can be done 


MOPeCO@rainate Gnly (the results for the y coordinate are 


Similar), using the method of "White noise model with adjusta- 


ble level." 
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The quality o£ the pertormanGeger thie cesgece sma a 7. 
lation can be observed from the following plots: 
a. True and estimated trajectoris of the target. 
b. Monte Carlo runs of position root mean square 


Error (seems coe 


an 30 gti ieee 4 
“Ogee 1 | 
d=) 
c. The same as b. £Ox the vele@ere ie cee oe ee 
2s VVanaly sas 


The target's movement is presented in Figure 6.26. 





Figure 6.26 Movement of the Manuevering Target 


iOS 


FOr tréating this problem, a linear Kalman Filter 


Gam S6euoca. Sine Only Gitference Bs that «he actual trajec- 


= 7 ~ 
\ 


B@meyeyOr ene ctarget 15 now given By several formulas instead 


7 


S=yone. The 4@#eferent ohases of movement of the ctarcet A,B, 


a 


C,D,8, can be formeac in oroper mathematical formulas, whicn 


can reed the position measurements one after the other, 


de 


GO 


eacwc Cn gene Samplinc time. 
ie Suse CStamatlesmeee cChNewstateSmi awe new eerwtScticr 


mere OoOteinec as: 


(00) = x4 (0 0) = 2, (9) or Se 

2(010) = %5 (0,0) = [2, (0) -2,(-1)]/T (6.84 
ace 

pe x (0) 4- x(0)T + °v, (-1) (nee) 

z, (0) = x(0) + v4 (0) (6.86) 

ae N(0,R,)) (Gert) 


- 


Tine Jmleiat Covariance MaEr1xX LS then: 


a Ri 4/1 
P(O|0) = (6.88) 
2 
yee 
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The rest of the calculations Meeded™ ome ieee 
the Kalman Filter algorithm are very similar to those 
developed in the first example. 

If the maneuvers are not taken into account and the 
filter runs as usual, the tracking results are very poor 
during the maneuvers (see pp. 114-115). 

Using the "White noise model with adjustable level" 
method, a self-adjustmemt of thewiwleer cao eecens 7-8 
as follows: 


After the square norm of innovations: 


exceeds a chosen value, the target 1s considered as maneuver- 
ing and a change (increase) in Q, 1s made. The increase 


in Q increases the gain G, so the state estimation: 
x(k|k) = X(K)K=1) + G(kizil =o ee (6.89) 


is more dependent on the second term since the state estimation 

x(k|k-1) is less reliable during the maneuver. The filter 

continues 1tS run with che Mews able soe.e) sine: Ey has again 

a lower value than that of the chosen threshold. Then the 

Filter runs again with the given initial value Gf 9 (O.—)0e0 
After the addition of the above provision in the 


tracking computer program, the results were much better than 


those of the non-adjustable filter. 


coe, 


Three Categories of resules were obtained. In 


- 
— 


wee) eer erenemvalues Of O'S anc =hreshola levels were 


Sacn 


used. 


The three categories are: 


Gm Olserved and estimated trajectories versus samoling 
time (50 Monte Carlo runs). 

Db. Observed and estimated trajectories versus samplings 
aime single. runs).. 

Peet icanesGuare position error (350 Monte Carlo runs). 


The above results 
B@eablilation 


developed as 


Symbols 
She 
1M: 
eM 


Meuy Good: 
Good: 

F. Noise: 

F. Noise E.: 
Deviation. 


Ss. Deviation. 


ne) 
b- 
t J 
LO 


(plots) can be seen on p> 
ena rmgudbietGaciOn Of Ehese results has 5Séen 


=OllLOows= 


Constant course and speed 
Ears: wManeuvier 
Second maneuver 


The estimated trajectory follows the time trajectory 
Verte credemnelutabll ley e(eoiner dence of the paths). 


The estimated trajectory is still very reliable, 
ine, LOltOwse ene true ipeen efesely but they 
Geom COINncldesexact ly. 


The estimated trajectory follows the noise. 


The estimated trajectory follows the noise 
exact lys 


Mierewas ea CevmaelOn between the trie sand estimated 
mrayecrory.. 


There 1s strong deviation between the true and 
estimated trajectory. 
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TAB GE se 
Observed and Estimated Trajectories 


(50 Monte Carlo Runs) 


RR. 
Q oe 3 20 
S Very Good Very Good Very Good 
Orel 1M Deviation S. Deviation S. Deviation 
2M S. Deviation S. Deviatroen S. Deviation 
S Very Good Very Good Good 
S 1M Good Deviation Deviation 
2M Deviation Deviation Deviation 
S Very Good Very Good Good 
10 1M Very Good Good Devrat Lom 
2M Good Deviation Deviation 
S Very Good Very Good Good 
100 1M Very Good Good Deviation 
2M Good Good Deviation 
S VenyeGood Very Good Dev taz1om 
1000 1M Very Good Very Good Deviation-~-F. Nemec 
2M Very Good Good Devlation--F= Neiiea 


0:9 


avr 


10 


100 


1000 


1M 


2M 


iM 


2M 


1M 


2M 


Very Good 


TABLE III 


bevwiat Lom 


S bow ae Om 


Ee Newse 


Good 


Good 


F. Noise 
Good 

Good 

F. Noise 
F. Noise 
F. Noise 
F. Noise 
F. Noise 
F. Noise 


Observed and Estimated Trajectoris 


(Single Runs) 


Very Good 
S. Deviation 


S. Deviation 


Very Good 
Good 


Bey tartan 


F. Noise 
Good 


Good 


Pe eNorse 
Good 


F. Noise 


F. Noise 
Good 


F. Noise 


EO 


ZU 


Very Gooc 
Deviation 


BleSwsl eis sWene 


Good 


Deviation 


DEViatLOn 


Good 


Devilation——-a- 


Deviation--F. 


F. Noise 


Deviation--F. 


Deviation--F. 


Noise 


Noise 


Noise 


Noise 


mel 


ht 


Symes 


IMAX: Maximum value Of M SvemmGueing them irce 


maneuver 

2MAX: Maximum value Of Mose 7m during ehevsecone 
maneuver 

3MIN: Minimium value of m.s.e. during movement 


under constant course and speed. 


MAN: Maneuvers 


le 


TABLE eV 
Ree@micag SQuard Sositicn Error {x Directio 


(50 Monte Carlo Runs) 


ee 

Q On i 3 20 
LMAX Doe 370 70) 
‘cae 2MAX 257 300 3740 
3MIN 2 30 30 
1MAX 82 lea g 455 
4 2MAX Hos 190 365 
3MIN 48 Bas 30 
TEMES 84 £30 A235 
10 2MAX 92 168 3410 
83MIN 52 aa 40 
LMAX 102 107 375 
100 2MAX 98 135 305 
3MIN 64 46 30 
LMAX 4 102 B45 
1000 2MAX 110 oe 295 
3MIN Te 51 30 

LMAX oO 2 

3000 2MAX —— ee 
3MIN 52 —— 


eZ 


TABLE 7 


Conclusions From Tablesmeew! 2i 1 
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Since the observed and estimated paths in the Monte 
Carlo case are averaged, the tracking results are better 
(smoother) and it 1S not easy to observe the various affec- 
BlemseOl extreme values of QO and the threshold level. For 
Poco redson, Only three characteristic plots of this case 
are presented and the following conclusions are based mainly 
Sm ehe Other two cases: 

a. When the target is moving with constant course and 
Speed, it is desirable to have low values of 9 and 
high threshold level. 

b. ‘When the target 1S maneuvering, it is desirable to 

have medium or high values of Q and low threshold 
level. 


Based on the above results, an attempt was made to 


run the filter using three levels of processnoise Q (two 


threshold levels). In this way the two types of maneuvers 
(slow-fast) were treated separately. The threshold levels 
chosen were 1 and 5. The mean square error of position and 


Pewecltey im che x Girection over 50 Monte Carlo runs was 
pileeread. The results (pp. 152-171) were very good especially 
for the position m.s.e. A tabulation of these results follows 


as Table VI. 


P10 


TABLE VI 


Multiple Process Noise Levels Results 


R.M.S. Posile#en igmer R.M.S. Vel@etey Euaen 


THR. 
Q’ 1-5 1-5 
LMAX 112 eae 
07Or1 le 2MAX 140 14 
3MIN 35 Qo 
LMAX 94 on 
03,180 2MAX 124 13.6 
3MIN 45 ir. 0 
LMAX 88 9.6 
0,103 fe0 2MAX 102 iia 29 
3MIN 50 eed 
LMAX 160 ied 
OQ ; 0 aan 2MAX 208 ine 6 
3MIN 35 0.4 
1MAX leis 10 
0, 1 ane 2MAX 168 ieee 
3MIN 40 0.6 
1MAX 96 10 
0) 2 eano 2MAX 140 ieee 
3MIN 44 0.8 
1MAX 136 Pils 
0 CO Sieacd 2MAX 140 ccm 
3MIN ou 0.25 
1MAX Vie 16 
©, | sew 2MAX 130 eye 
3MIN 44 0.7 
1MAX 100 13.5 
Ome Lee 2MAX 120 15.3 
3MIN 46 ie 0 
LMAX 89 in@neeG 
Oy 1 Ope oe 2MAX 106 1S 
3MIN 50 3 
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oe) Cemeaectson With Other Aporoaches 

Sea oinom) | Rete Glee nasmecveloned 4a Gizterent approach 
Peamede Sane PEODLen.@e ln this case the trackine filter 
Operates in its normal mode in the absence of any maneuver. 
Once a maneuver 1s detected, a different state model is usec 
by the filter. The acceleration is added as a new state 
component. The extent of the maneuver as detected, is tnen 
Usea tO yield an estimate for the extra state component, 
Seiemeoerect lions are mademen the other state components. The 
tracking 1s then done with the augmented state modei until 
Pee gee be COnvenreEca te the normal model py another cdecision. 

Miemses ees Or Melis adoorOach Can be seen in Figure 
6.65.  8fhe Curve having thew™indication VD’ belongs to the 
Boe so eo aleniiewillec tne Other cusve (indication "IE”") 
belongs to a different approach developed by Chan [Ref. 10]. 
ive weibtnesobnene latter algorithm is the following: 

When a maneuver is detected, the magnitude of the 
Beeceolocaelonewraridenmtiricdwin a least squares format. The 
as eemeUSed in Conjunction witn a standard Kalman Filter 
to estimate the state of the vehicle. The aim of the accelera- 
PPomerneMeresSeimatien 1S to remove the filter bias caused 
Bye dew eeraeecemecv ating £4om Ege assumed constant velocity, 
Seco Wie nemmo t Lon. 

merGammecrecasilyesee (Pigure 6.83) that the algorithm 
of Reference 9 1s Superior to that of Reference 10. 


The "white noise model with adjustable level" method, 


modified with multiple levels of process noise Q (one for 
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each type of maneuver), can be compared with the aigoritnms 

of References 9 and 10. Specifically ehise Spacers algoriétnm 
Pech or ela DOLImEne algom chica in the @e.m.Ss cOSition 

SS eeereee Nene cmmeme  VOLSS ti eae e-m.S. Velocity error resulcs. 
Pemeene 6. s. Velocity error case, the approximate maximum 

Wemue Of the ewo Other algoritnms is ll, while this paper's 
algorithm maximum value is 13.3 (not too big a dcifierence; 


iM~per= F.M.Ss position error case, the maximum values for 


(D 


a “ 


awe eeeee> Janda LOwalgortenms are 200 and 125, respectiv 
while this paper's algorithm has acnieved an approximate 


value of 95. 
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VILI.~ CONGEUSTOMS 


In the examples presented, 1t was seen that the Kalman 
Filter has the ability and flexibi |e Beem trea eee 
cases of the target tracking problem with very satisfactory 
results. 

The Kalman Filter is alwayseimitialezeqmoys toomiaei 
providing the initial estimate x (K) and “BES. corvespending 
estimate error covariance Matvix P(kK)— ORS ia ees 
is of great importance for the Eiltemes peviormanca. some ee 
initialization needs more Observations sac 2 oer 
algorithm estimate to converge toward the value of the state 
vector. 

In all cases the importance of the Gain matn1* 7G een 
the Kalman Filter was significant, 1.e., Since it is inversely 
proportional to time, 1t welGhts the copeectiene cea 
ee ee ce less heavily as time progresses, and so the 
state estimation x (k|k) depends more on the state estimation 
of past time ee) 

It was also noted that the Q matrix accounts for any 
model inaccuracies. For a filter in a steady yState Condicionm 
the Q also serves to prevent the gain matrix G(k) from 
approaching zero by always insuring uncertainty in the pre- 
dicted error covariance matrix. Q is also the key variable 


for treating the maneuvering target problem with the "White 


les 


noise model with adjustable level" method. As it was ex- 
SeoiMed In ene eorresoonding chapter, variations in QO cause 
pee eel Elona! Vamtartons £O theegain G(k), which weights the 
B@acece lon facecen in the equation: 


ss “~N wn 


eee) eK 1) 0G ik) [ztke) clk) x(4Bk-1) ) 


t th 
fu) 


Larger values of 9 tend to cause the state estimate to 
ENemompservatlion. 

Mi-—-o Grom covarlanice Natwix and specirically some of 
its elements (position and velocity error variance), were 
very userul as indicators of the filter's performance. 

In the case of the maneuvering target, the "White noise 
model with adjustable level" method was very reliable ana 
Selo wedete tO Ene algorltnmswemereferences 9 and 10. Addi- 
Pousti ceils Much Simpler than the other two methods. The 
STP yeetoacdcvantage was the greater value of the velocity r.m-s. 
error. In the example presented, the difference was not 
PacieeereaQe Ut eic 1S Gstimated thate)it would increase in 
cases where the target maneuvers more drastically (greater 
SCGemeratiens) - 

The Monte Carlo simulation should be used to provide 
Seperate oles FOr any stocalhistic process which is 
represented by pseudorandom numbers. This simulation 1s a 
Bie eeeUewotetstical procedure, tO compare two or more 


ale@mrehnss 


Lic 


More specific conclusions concerning the examples presented 
nave already been provided in the corresponding analyses of 
them. 

Tt 1S suggested that a Gontinvattentest seks seco anaes 
should include the feeble wine: 

a. Problem of maneuvering target which uses greater 
acceleration driving the maneuvers. The "White noise 
model with adjustable level” method should be applied and 
the results 1n poSition and velocity ©2mM.s] Gio nse gem ee 
obtained. 

b. Problem of multiple taxngets Undew Vayiteus 42 44. 
(maneuvering weinec) ut bem eee 

c. Problem of single and multiple maneuvering targets 
in clutter. It 1S an interesting and ipemeaneweoe le ere 
included in thus paper dwe-cemiack soreean=- 

d. Study the implementation of a Kalman Filter algorithm 


uSing special purpose hardware. 
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